// Included files
#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/delay.h>
#include "oi.h"
#include <stdio.h>
#include <stdlib.h>


// Constants
#define USB 					1 //change serial to computer and create
#define CR8 					2 //change serial to computer and create

#define RESET_SONG 		0
#define START_SONG 		1
#define BUMP_SONG  		2
#define END_SONG   		3

#define LEDPower 			0xFE
#define LEDAll 				0xFF

#define InitialSweep	0
#define GoToWall			1
#define ReachWall			2
#define FollowWall		3
#define EndMapping		4



// Global variables
volatile int16_t ADC_C5 = 0;

volatile uint16_t timer_cnt = 0;
volatile uint8_t timer_on = 0;

volatile uint8_t sensors_flag = 0;
volatile uint8_t sensors_index = 0;
volatile uint8_t sensors_in[Sen6Size];//consolidate thease
volatile uint8_t sensors[Sen6Size];

volatile int16_t distance = 0;
volatile int16_t angle = 0;
volatile int16_t sweep_current_angle = 0;
volatile	int16_t	start_to_wall = 0;
volatile	int16_t	distance_from_first_wall = 0;
volatile	int16_t	distance_from_second_wall = 0;
volatile	int16_t	distance_from_third_wall = 0;
volatile	int16_t	distance_from_fourth_wall = 0;

//LED state 
//uint8_t Current_Power_LED_Color = 0x00; // 0x00 = Green, 0xFF = Orange
//uint8_t Current_Power_LED_Intensity = 0x00; // 0x00 = off, 0xFF = full
//uint8_t Current_Play_LED_State = 0x00; // 0x00 = off, 0x02 = on
//uint8_t Current_Advance_LED_State = 0x00; // 0x00 = off, 0x08 = on

// Functions
void initialize(void);
void defineSongs(void);
void baud(uint8_t baud_code);
void powerOnRobot(void);
void powerOffRobot(void);

void setSerial(uint8_t com); 
uint8_t getSerialDestination(void); 
void byteTx(uint8_t value);
uint8_t byteRx(void);
void flushRx(void);
void sendSensorPacketOverUSB(uint8_t packetNumber, uint8_t packetSize);
void writeInt(uint8_t i, uint8_t com);

void flashLED(uint8_t LED, uint16_t delay, uint16_t Color, uint16_t Intensity);

void drive(int16_t velocity, int16_t radius);
uint16_t randomAngle(void);
void RotateCW(void);
void RotateCCW(void);
void Stop(void);
void DriveStraight(void);

void delayMs(uint16_t time_ms);
void delayAndUpdateSensors(unsigned int time_ms);
void delay10ms(uint16_t delay_10ms);

int16_t GetNearestWall(int16_t SeedValue, int16_t Measurements, int16_t Sensor);
int16_t GetFollowingThatWall(uint16_t wallDistance, int16_t NumMeasurements);
int16_t GetAnalogReading(uint8_t NumReads, uint8_t pin);
int16_t GetEZ4Reading(uint8_t readAverage);//antiquated replaced by GetAnalogReading(uint8_t NumReads, uint8_t pin);

int16_t GetToWall(uint16_t NumSteps);







int main (void) 
{

  // Set up Create and module
  initialize();
  LEDBothOff;
  powerOnRobot();
  byteTx(CmdStart);
  baud(Baud57600);
  defineSongs();
  byteTx(CmdControl);
  byteTx(CmdFull);

  // Stop just as a precaution
  drive(0, RadStraight);

  // Play the reset song and wait while it plays
  byteTx(CmdPlay);
  byteTx(RESET_SONG);
  delayAndUpdateSensors(750);
	

			
			
int16_t current_sweep_reading = 0;

/*	
      for (;;)
      {
					current_sweep_reading = GetAnalogReading(10, 7); //read distance to wall at current increment
					writeInt(current_sweep_reading, USB);//only two needed
					writeInt(current_sweep_reading>>8, USB);
					delayAndUpdateSensors(750);
					
					current_sweep_reading = GetAnalogReading(10, 5); //read distance to wall at current increment
					writeInt(current_sweep_reading, USB);//only two needed
					writeInt(current_sweep_reading>>8, USB);
					delayAndUpdateSensors(750);
			}			

	      for (;;)
      {
					current_sweep_reading = GetEZ4Reading(10); //read distance to wall at current increment/
					writeInt(current_sweep_reading, USB);//only two needed
					writeInt(current_sweep_reading>>8, USB);
					delayAndUpdateSensors(750);
			}

*/

start_to_wall = GetNearestWall(255, 60, 5);
distance_from_first_wall = GetToWall(255);//if it cant get there in 255 measurements somthings wrong or it is in a big friggen room. update with a loop or aux sensor check
distance_from_second_wall = GetFollowingThatWall(distance_from_first_wall, 10);
distance_from_third_wall = GetFollowingThatWall(distance_from_second_wall, 10);
distance_from_fourth_wall = GetFollowingThatWall(distance_from_third_wall, 10);


}




int16_t GetFollowingThatWall(uint16_t wallDistance, int16_t NumMeasurements)
{
	int16_t	sweep_reading = 255;
	int16_t	distance_to_wall_to_follow = wallDistance;
	int16_t count = 0;
	int16_t CorrectionCW = 0;
	int16_t CorrectionCCW = 0;	
	
	//find wall with sensor 7. hopfully rotate 90 degrees clockwise
	drive(50, RadCW);
	delayAndUpdateSensors(1200);
	Stop();
	distance_to_wall_to_follow = GetNearestWall(wallDistance, NumMeasurements, 7);
	
	
	
	for(count = 0; count < 222 ; count++)//avoid the dreaded infinite loop we were having
	{
	
		//now we are trying to find the wall in front of sensor 5
		sweep_reading = GetToWall(0);
		if(sweep_reading < 10)
		{
				break;
		}
		
	
		//read the wall next to us in front of side sensor 7
		sweep_reading = GetAnalogReading(15, 7);
		
		//if on course keep going
		if(sweep_reading == distance_to_wall_to_follow)
		{
				continue;
		}
		//otherwise adjust
		else if(sweep_reading < distance_to_wall_to_follow - 2)
		{
				
				if(CorrectionCW == 0)
				{
					drive(50, RadCW);
					delayAndUpdateSensors(500);
					Stop();
				}
				CorrectionCW++;
				if(CorrectionCW == 4)
				{
					CorrectionCW = 0;
				}
				
		}
	
		else if(sweep_reading > distance_to_wall_to_follow + 2)
		{
				if(CorrectionCCW == 0)
				{
					drive(50, RadCCW);
					delayAndUpdateSensors(500);
					Stop();
				}
				CorrectionCCW++;
				if(CorrectionCCW == 4)
				{
					CorrectionCCW = 0;
				}
		}
	
	}
	
	
	return GetAnalogReading(10, 5);

}








int16_t GetToWall(uint16_t NumSteps)
{
	int16_t count = 0;
	int16_t	sweep_reading = 255;  


	for (count = 0; NumSteps >= count; count++)
	{
		sweep_reading = GetAnalogReading(15, 5);
		//writeInt(sweep_reading, USB);//only two needed
		//writeInt(sweep_reading>>8, USB);
		if(sweep_reading < 10)
		{
		  break;
		}
		
		if(sensors[SenBumpDrop] & BumpEither)  // Check for a bump
    {
			drive(-50, RadStraight);
			delayAndUpdateSensors(250);
			Stop();
      // Set the turn parameters and reset the angle
      if(sensors[SenBumpDrop] & BumpLeft)
			{
				drive(50, RadCW);
				delayAndUpdateSensors(500);
				Stop();
			}
			// Set the turn parameters and reset the angle
      else if(sensors[SenBumpDrop] & BumpRight)
			{
				drive(50, RadCCW);
				delayAndUpdateSensors(500);
				Stop();
			}
			else
			{
				drive(-50, RadStraight);
				delayAndUpdateSensors(500);
				Stop();
			}
		}
		
		drive(45 + sweep_reading, RadStraight);
		delayAndUpdateSensors(500);
		Stop();

	}
	return sweep_reading;
}














/*
	int16_t SeedValue = known wall value EZ4 max is 255 if unknown dont really have this really useful yet
	int16_t Measurements = how many sensor values to average during measurements
	int16_t Sensor = which sensor to use 

*/

int16_t GetNearestWall(int16_t SeedValue, int16_t Measurements, int16_t Sensor)
{
	int16_t	short_sweep_reading = 255;//SeedValue; 
  int16_t	last_sweep_reading = 255; 
	int16_t	sweep_reading = 255;  
  int16_t count = 0;
	int16_t short_count = 0;
	int16_t number_of_measurements = Measurements;
	
	for (count = 0; number_of_measurements >= count-1; count++)// do a full circle and some more in case we are on carpet. take measurements, record shortest
	{
		drive(50, RadCW);
		delayAndUpdateSensors(500);
		Stop();
		sweep_reading = GetAnalogReading(15, Sensor);
		if(sweep_reading < short_sweep_reading)
		{
			short_sweep_reading = sweep_reading;
		}
	}

	
	
	for (count = 0; count < 100; count++)
	{
		if(count != 0)//dont need the go on first iteration in case it is the shortest
		{
			drive(50, RadCCW);
			delayAndUpdateSensors(500);
			Stop();
		}
		
		sweep_reading = GetAnalogReading(15, Sensor);
		if(sweep_reading <= short_sweep_reading+4 )// Turn back in steps till find within a tolerance of 4 inches of shortest
		{
		  
			for (short_count = 0; count < 150; count++)// small measurements now that we are in 4 inch tolerance
			{
				drive(50, RadCCW);
				delayAndUpdateSensors(250);
				Stop();
				last_sweep_reading = sweep_reading;
				sweep_reading = GetAnalogReading(15, Sensor);
				
				if(sweep_reading == last_sweep_reading )//count how many rotations are the same distance
				{
					short_count++;
				}
				
				if(sweep_reading < last_sweep_reading )// if a smaller measurement is found start over
				{
					short_count = 0;
				}
				
				if(sweep_reading > last_sweep_reading )// if measurement is larger then go back 
				{
					for (count = 0; count < (short_count/2); count++)//half the distance of the shortest series of measurements should be the middle
					{
						drive(50, RadCW);
						delayAndUpdateSensors(250);
						Stop();
						delayAndUpdateSensors(250);
					}
					count = 200;
					break;
				}

			}
		}

	}
	Stop();
  // Play the reset song and wait while it plays
  byteTx(CmdPlay);
  byteTx(RESET_SONG);
  delayAndUpdateSensors(750);

	return (last_sweep_reading + GetAnalogReading(15, Sensor))/2;

}
























int16_t GetAnalogReading(uint8_t NumReads, uint8_t pin)
{
	int16_t aveRead = 0;

		/* Set up the ADC on pin  */
		//DDRC &= ~0xC30;
		DDRC &= ~0x20;
		PORTC &= ~0x20;	
			
		/* Disable digital input on  */
		DIDR0 |= 0x20;
	  //DIDR0 |= 0x30;
		
		/* 
		Disable ADC power reduction  Bit 0 PRADC: Power Reduction ADC Writing 
		a logic one to this bit shuts down the ADC. The ADC must be disabled 
		before shut down. The analog comparator cannot use the ADC input 
		MUX when the ADC is shut Down
		*/
		PRR &= ~0x01; 
		//PRR &= ~_BV(PRADC); // Turn off ADC power save
		
		MCUCR &= ~0x10;
		
		/* 
		enable ADC bit 7
		prescale = 128  bits 2:0 Determine the division factor between 
		system clock frequency and the input clock to the ADC.
		*/
		ADCSRA = 0x87;
		//ADCSRA = (_BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0)); // Enabled, prescaler = 128
		
		/* set the ADC channel to C5 */
		//ADMUX |= 0x05; 
		//ADMUX = (_BV(REFS0) | 0x05); // set voltage reference, also select channel C5	
			
		/* Set Voltage reference 
		AVcc with external capacior at AREF pin bits 7:6 = 10
		*/
		ADMUX |= 0x40; 

		/* to left adjust result set pin 5 to 1 */
		//ADMUX |= 0x10;
			
		for(int i = 0; i <= NumReads; i++)
		{
			delayAndUpdateSensors(49);
			/* clear the ADC channel*/
			ADMUX &= ~0x0F; 
			
			/* set the ADC channel to c5*/
			ADMUX = (ADMUX & 0xf0) | (pin & 0x0f);  
	
			/* start the ADC */
			ADCSRA |= 0x40; 
				
			/* Wait until its finished */
			while(ADCSRA & 0x40) 
				
			/* save the result */
			//ADC_C5 = ADC;
			ADC_C5 = ADC*0.48828125;
			aveRead += ADC*0.48828125;
			flushRx();
			ADC = 0;
		}
		ADC_C5 = aveRead/NumReads;

	return ADC_C5;


	
}


















int16_t GetEZ4Reading(uint8_t readAverage)
{
		int16_t aveRead = 0;
		/* Set up the ADC on pin  */
		//DDRC &= ~0xC30;
		DDRC &= ~0x20;
		PORTC &= ~0x20;	
			
		/* Disable digital input on  */
		DIDR0 |= 0x20;
	  //DIDR0 |= 0x30;
		
		/* 
		Disable ADC power reduction  Bit 0 PRADC: Power Reduction ADC Writing 
		a logic one to this bit shuts down the ADC. The ADC must be disabled 
		before shut down. The analog comparator cannot use the ADC input 
		MUX when the ADC is shut Down
		*/
		PRR &= ~0x01; 
		//PRR &= ~_BV(PRADC); // Turn off ADC power save
		
		MCUCR &= ~0x10;
		
		/* 
		enable ADC bit 7
		prescale = 128  bits 2:0 Determine the division factor between 
		system clock frequency and the input clock to the ADC.
		*/
		ADCSRA = 0x87;
		//ADCSRA = (_BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0)); // Enabled, prescaler = 128
		
		/* set the ADC channel to C5 */
		//ADMUX |= 0x05; 
		//ADMUX = (_BV(REFS0) | 0x05); // set voltage reference, also select channel C5	
			
		/* Set Voltage reference 
		AVcc with external capacior at AREF pin bits 7:6 = 10
		*/
		ADMUX |= 0x40; 

		/* to left adjust result set pin 5 to 1 */
		//ADMUX |= 0x10;
			
		for(int i = 0; i <= readAverage; i++)
		{
			delayAndUpdateSensors(49);
			/* clear the ADC channel*/
			ADMUX &= ~0x0F; 
			
			/* set the ADC channel to c5*/
			ADMUX |= 0x05; 
	
			/* start the ADC */
			ADCSRA |= 0x40; 
				
			/* Wait until its finished */
			while(ADCSRA & 0x40) 
				
			/* save the result */
			//ADC_C5 = ADC;
			ADC_C5 = ADC*0.48828125;
			aveRead += ADC*0.48828125;
			flushRx();
			ADC = 0;
		}
		ADC_C5 = aveRead/readAverage;
		return ADC_C5;

}











// Initialize the Mind Control's ATmega168 microcontroller
void initialize(void)
{

	//
	// Turn off interrupts
  cli();

  // Configure the I/O pins
	//DDRA  = 0x00;
  //PORTA = 0x00;
  DDRB = 0x10;
  PORTB = 0xCF;
	DDRC    = 0x00;//  DDRC = 0x00;
  PORTC   = 0xFF;//  PORTC = 0xFF;
  DDRD = 0xE6;
  PORTD = 0x7D;

  // Set up timer 1 to generate an interrupt every 1 ms
  TCCR1A = 0x00;
  TCCR1B = (_BV(WGM12) | _BV(CS12));
  OCR1A = 71;
  TIMSK1 = _BV(OCIE1A);

  // Set up the serial port with rx interrupt
  UBRR0 = 19;
  UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));
  UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));

  // Turn on interrupts
  sei();
}


// Serial receive interrupt to store sensor values
SIGNAL(SIG_USART_RECV)
{
  uint8_t temp;


  temp = UDR0;

  if(sensors_flag)
  {
    sensors[sensors_index++] = temp;
    if(sensors_index >= Sen6Size)
      sensors_flag = 0;
  }
}




// Timer 1 interrupt to time delays in ms
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
  if(timer_cnt)
    timer_cnt--;
  else
    timer_on = 0;
}

void RotateCCW(void)
{
		//RotateCCW.
		byteTx(145);
		byteTx(0);
		byteTx(100);
		byteTx(0xFF);
		byteTx(-100);
}

void RotateCW(void)
{
		//RotateCW.
		byteTx(145);
		byteTx(0xFF);
		byteTx(-100);
		byteTx(0);
		byteTx(100);
}

void DriveStraight(void)
{
	drive(300, RadStraight);
}

void Stop(void)
{
  drive(0, RadStraight);
}







// Send Create drive commands in terms of velocity and radius
void drive(int16_t velocity, int16_t radius)
{
  byteTx(CmdDrive);
  byteTx((uint8_t)((velocity >> 8) & 0x00FF));
  byteTx((uint8_t)(velocity & 0x00FF));
  byteTx((uint8_t)((radius >> 8) & 0x00FF));
  byteTx((uint8_t)(radius & 0x00FF));
}




// Return an angle value in the range 53 to 180 (degrees)
uint16_t randomAngle(void)
{
    return (53 + ((uint16_t)(random() & 0xFF) >> 1));
}



// Delay for the specified time in ms without updating sensor values
void delayMs(uint16_t time_ms)
{
  timer_on = 1;
  timer_cnt = time_ms;
  while(timer_on) ;
}

// Delay for the specified time in ms and update sensor values
void delayAndUpdateSensors(uint16_t time_ms)
{
  uint8_t temp;

  timer_on = 1;
  timer_cnt = time_ms;
  while(timer_on)
  {
    if(!sensors_flag)
    {
      for(temp = 0; temp < Sen6Size; temp++)
        sensors[temp] = sensors_in[temp];

      // Update running totals of distance and angle
      distance += (int)((sensors[SenDist1] << 8) | sensors[SenDist0]);
      angle += (int)((sensors[SenAng1] << 8) | sensors[SenAng0]);

      byteTx(CmdSensors);
      byteTx(CR8Sen6);
      sensors_index = 0;
      sensors_flag = 1;
    }
  }
}




/* delay in 10 ms increments
LED Bit Masks
LEDPower        0xFF
LEDAdvance      0x08
LEDPlay         0x02
LEDsBoth        0x0A
LED1            0x20
LED2            0x40
LEDBoth         0x60
*/
void flashLED(uint8_t LED, uint16_t delay, uint16_t Color, uint16_t Intensity)
{
	if (LED == LEDBoth) // Command Module
	{
		LEDBothOff;
		delay10ms(delay);
		LEDBothOn;
		delay10ms(delay);
		LEDBothOff;
	}
	else if (LED == LED1) // Command Module
	{
		LED1Off;
		delay10ms(delay);
		LED1On;
		delay10ms(delay);
		LED1Off;
	}
	else if (LED == LED2) // Command Module
	{
		LED2Off;
		delay10ms(delay);
		LED2On;
		delay10ms(delay);
		LED2Off;
	}
	else if (LED == LEDPower) // Create 
	{
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
		delay10ms(delay);
		byteTx(139);
		byteTx(0);
		byteTx(Color);
		byteTx(Intensity);
		delay10ms(delay);
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
	}	
	else if (LED == LEDAdvance) // Create 
	{
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
		delay10ms(delay);
		byteTx(139);
		byteTx(8);
		byteTx(0);
		byteTx(0);
		delay10ms(delay);
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
	}	
	else if (LED == LEDPlay) // Create 
	{
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
		delay10ms(delay);
  	byteTx(139);
		byteTx(2);
		byteTx(0);
		byteTx(0);
		delay10ms(delay);
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
	}	
	else if (LED == LEDAll) // Create and Command Module
	{
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
		LEDBothOff;
		delay10ms(delay);
		LEDBothOn;
  	byteTx(139);
		byteTx(10);
		byteTx(Intensity);
		byteTx(delay);
		delay10ms(delay);
		LEDBothOff;
		byteTx(139);
		byteTx(0);
		byteTx(0);
		byteTx(0);
	}	
}








void sendSensorPacketOverUSB(uint8_t packetNumber, uint8_t packetSize)
{
	uint8_t startSerialState = getSerialDestination();
	uint8_t count;
	uint8_t packet[packetSize];  /* array for sensor data */
	

	
	if((packetNumber == 0) || (packetNumber == 1) || (packetNumber == 2) || (packetNumber == 3) || (packetNumber == 4) || (packetNumber == 5) || (packetNumber == 6))
	{ 
		
		byteTx(CmdSensors); /* sensor opcode*/
		byteTx(packetNumber); /* send request for packet */		
	
		for(count = 0; count < packetSize; count++)
		{
			packet[count] = byteRx();/* Put sensor data in command module memory */		
		}
		
		for(count = 0; count < packetSize; count++)
		{
		
			//writeInt(itoa(packet[count], buf, 10), USB);	/* Write sensor data to XBEE */
			writeInt(packet[count], USB);	/* Write sensor data to XBEE */		
		}
		setSerial(startSerialState);
		
	}
	
	
	if(packetNumber == 7)
	{ 
	
		/* Set up the ADC on pin  */
		//DDRC &= ~0xC30;
		DDRC &= ~0x20;
		PORTC &= ~0x20;	
			
		/* Disable digital input on  */
		DIDR0 |= 0x20;
	  //DIDR0 |= 0x30;
		
		/* 
		Disable ADC power reduction  Bit 0 PRADC: Power Reduction ADC Writing 
		a logic one to this bit shuts down the ADC. The ADC must be disabled 
		before shut down. The analog comparator cannot use the ADC input 
		MUX when the ADC is shut Down
		*/
		PRR &= ~0x01; 
		//PRR &= ~_BV(PRADC); // Turn off ADC power save
		
		MCUCR &= ~0x10;
		
		/* 
		enable ADC bit 7
		prescale = 128  bits 2:0 Determine the division factor between 
		system clock frequency and the input clock to the ADC.
		*/
		ADCSRA = 0x87;
		//ADCSRA = (_BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0)); // Enabled, prescaler = 128
		
		/* set the ADC channel to C5 */
		//ADMUX |= 0x05; 
		//ADMUX = (_BV(REFS0) | 0x05); // set voltage reference, also select channel C5	
			
		/* Set Voltage reference 
		AVcc with external capacior at AREF pin bits 7:6 = 10
		*/
		ADMUX |= 0x40; 

		/* to left adjust result set pin 5 to 1 */
		//ADMUX |= 0x10;
			
		/* clear the ADC channel*/
		ADMUX &= ~0x0F; 
		
		/* set the ADC channel to c5*/
		ADMUX |= 0x05; 

		/* start the ADC */
		ADCSRA |= 0x40; 
			
		/* Wait until its finished */
		while(ADCSRA & 0x40) 
			
		/* save the result */
		//ADC_C5 = ADC;
		ADC_C5 = ADC*0.48828125;
		
		//ADC_C5 = 171;
		//writeInt(ADC_C5, USB);//only two needed
		//writeInt(ADC_C5>>8, USB);
		//writeInt(' ', USB);
		 
		char buf[5];
		itoa(ADC_C5, buf, 10);
		uint8_t count;

		for(count = 0; count <= 5; count++)
		{
			writeInt(buf[count], USB);	
			buf[count] = 0;
		}
		flushRx();
		ADC = 0;
		setSerial(startSerialState);

	}

}




// Switch the baud rate on both Create and module
void baud(uint8_t baud_code)
{
  if(baud_code <= 11)
  {
    byteTx(CmdBaud);
    UCSR0A |= _BV(TXC0);
    byteTx(baud_code);
    // Wait until transmit is complete
    while(!(UCSR0A & _BV(TXC0))) ;

    cli();

    // Switch the baud rate register
    if(baud_code == Baud115200)
      UBRR0 = Ubrr115200;
    else if(baud_code == Baud57600)
      UBRR0 = Ubrr57600;
    else if(baud_code == Baud38400)
      UBRR0 = Ubrr38400;
    else if(baud_code == Baud28800)
      UBRR0 = Ubrr28800;
    else if(baud_code == Baud19200)
      UBRR0 = Ubrr19200;
    else if(baud_code == Baud14400)
      UBRR0 = Ubrr14400;
    else if(baud_code == Baud9600)
      UBRR0 = Ubrr9600;
    else if(baud_code == Baud4800)
      UBRR0 = Ubrr4800;
    else if(baud_code == Baud2400)
      UBRR0 = Ubrr2400;
    else if(baud_code == Baud1200)
      UBRR0 = Ubrr1200;
    else if(baud_code == Baud600)
      UBRR0 = Ubrr600;
    else if(baud_code == Baud300)
      UBRR0 = Ubrr300;

    sei();

    delay10ms(10);
  }
}







void powerOnRobot(void)
{
  // If Create's power is off, turn it on
  if(!RobotIsOn)
  {
      while(!RobotIsOn)
      {
          RobotPwrToggleLow;
          delay10ms(50);  // Delay in this state
          RobotPwrToggleHigh;  // Low to high transition to toggle power
          delay10ms(10);  // Delay in this state
          RobotPwrToggleLow;
      }
      delay10ms(350);  // Delay for startup
  }
}

void powerOffRobot(void)
{
  // If Create's power is on, turn it off
  if(RobotIsOn)
  {
      while(RobotIsOn)
      {
          RobotPwrToggleLow;
          delay10ms(50);  // Delay in this state
          RobotPwrToggleHigh;  // Low to high transition to toggle power
          delay10ms(10);  // Delay in this state
          RobotPwrToggleLow;
					delay10ms(350);
      }
      delay10ms(350);  // Delay for startup
  }
}


void setSerial(uint8_t com)
{
 if(com == USB)
 PORTB |= 0x10;
 else if(com == CR8)
 PORTB &= ~0x10;
}



uint8_t getSerialDestination(void)
{
 if (PORTB & 0x10)
   return USB;
 else
   return CR8;
}

void delay10ms(uint16_t delay_10ms)
{
  // Delay for (delay_10ms * 10) ms
  while(delay_10ms-- > 0)
  {
    // Call a 10 ms delay loop
    _delay_loop_2(46080);
  }
}

uint8_t byteRx(void)
{
  // Receive a byte over the serial port (UART)
  while(!(UCSR0A & _BV(RXC0))) ;
  return UDR0;
}




void flushRx(void)
{
  uint8_t temp;

  // Clear the serial port
  while(UCSR0A & _BV(RXC0))
    temp = UDR0;
}




void byteTx(uint8_t value)
{
  // Send a byte over the serial port
  while(!(UCSR0A & _BV(UDRE0))) ;
  UDR0 = value;
}


void writeInt(uint8_t i, uint8_t com)
{
 uint8_t originalDestination = getSerialDestination();
 if (com != originalDestination)
 {
    setSerial(com);
    delay10ms(1);
 }
 byteTx((uint8_t)(i));
 delay10ms(1); // Allow char to xmt.
 if (com != originalDestination)
 {
    setSerial(originalDestination);
    delay10ms(1);
 }
}



// Define songs to be played later
void defineSongs(void)
{
  // Reset song
  byteTx(CmdSong);
  byteTx(RESET_SONG);
  byteTx(4);
  byteTx(60);
  byteTx(6);
  byteTx(72);
  byteTx(6);
  byteTx(84);
  byteTx(6);
  byteTx(96);
  byteTx(6);

  // Start song
  byteTx(CmdSong);
  byteTx(START_SONG);
  byteTx(6);
  byteTx(69);
  byteTx(18);
  byteTx(72);
  byteTx(12);
  byteTx(74);
  byteTx(12);
  byteTx(72);
  byteTx(12);
  byteTx(69);
  byteTx(12);
  byteTx(77);
  byteTx(24);

  // Bump song
  byteTx(CmdSong);
  byteTx(BUMP_SONG);
  byteTx(2);
  byteTx(74);
  byteTx(12);
  byteTx(59);
  byteTx(24);

  // End song
  byteTx(CmdSong);
  byteTx(END_SONG);
  byteTx(5);
  byteTx(77);
  byteTx(18);
  byteTx(74);
  byteTx(12);
  byteTx(72);
  byteTx(12);
  byteTx(69);
  byteTx(12);
  byteTx(65);
  byteTx(24);
}
